#include "path.h"

/**
 * \brief 成员函数定义
 * \name PATH
 */
void path::generate_head()
{
    // 获取当前位置并生成路径点
    vpoints.emplace_back(car_cur.x, car_cur.y, 1);
    vpoints.emplace_back(car_cur.x, car_cur.y + 0.3, 1);
    vpoints.emplace_back(car_cur.x / 2, car_cur.y + 0.6, 1);
    vpoints.emplace_back(0.2, -0.7, 1);
    vpoints.emplace_back(0.2, -0.21, 1);
}

void path::generate_back()
{
    // 获取当前位置并生成路径点
    vpoints.emplace_back(car_cur.x, car_cur.y, 1);
    vpoints.emplace_back(car_cur.x, car_cur.y + 0.3, 1);
    vpoints.emplace_back(car_cur.x / 2, car_cur.y + 0.6, 1);
    vpoints.emplace_back(0.05, -0.7, 1);
    vpoints.emplace_back(0.05, -0.21, 1);
}

void path::generate_side()
{
    // 获取当前位置并生成路径点
    vpoints.emplace_back(car_cur.x, car_cur.y, 1);
    vpoints.emplace_back(car_cur.x, car_cur.y + 0.2, 1);
    vpoints.emplace_back(car_cur.x / 2, car_cur.y + 0.6, 1);
    vpoints.emplace_back(0.15, -0.7, 1);
    vpoints.emplace_back(0.08, -0.3, 1);
}

int path::generate(my_msg::LaserScans* msg)
{
    if ( get_position(msg) == 0 ) {
        return 0;
    }
    switch ( type ) {
    case HEAD:
        generate_head();
        break;
    case BACK:
        generate_back();
        break;
    case SIDE:
        generate_side();
        break;
    default:
        cout << "\033[31m\033[45mERROR: path type is wrong!"
             << "\033[0m" << endl;
    }
    generate_vpoints(10);
    for ( auto point : vpoints ) {
        cout << point.x << ",";
    }
    cout << endl;
    cout << endl;
    cout << endl;
    cout << endl;
    for ( auto point : vpoints ) {
        cout << point.y << ",";
    }
    generate_dvp();

    cout << "generate is done !" << endl;
}

/**
 * @param msg
 * @return 1 表示进行追踪算法\n
 *         0 表示重新进行定位
 */
int path::count_path(my_msg::LaserScans* msg)
{
    // 停车完成
    if ( flag == 2 ) {
        cout << "\033[31m\033[44mstop node is done!!!\033[0m" << endl;
        return 0;
    }
    // 获取当前位置
    if ( get_position(msg) == 0 ) {
        return 0;
    }

    double t;
    if ( type == SIDE ) {
        // 这里是侧方停车决策的地方
    }
    else if ( type == HEAD || type == BACK ) {
        if ( flag == 0 ) {
            cout << "trace_two_sign" << endl;
            t = trace_two_sign();
        }
        else if ( flag == 1 ) {
            cout << "trace_one_sign" << endl;
            t = trace_one_sign();
        }

        if ( type == HEAD ) {
            return 10 * (int)(t * 180 / Pi);
        }
        else {
            return -10 * (int)(t * 180 / Pi);
        }
    }
    return 0;
}

int path::get_position(my_msg::LaserScans* msg)
{
    vector<line> lines;
    int          len = msg->ranges.size();
    line*        t   = new line();
    /**************************************************************************
     * 将线段识别并添加进lines
     * 逻辑有点复杂，懒得写注释了
     */
    for ( unsigned long i = 0; i < len; i++ ) {
        double angle_t = -msg->angle_min - i * msg->angle_increment;
        if ( type == BACK ) {
            angle_t = angle_t > 0 ? angle_t - Pi : angle_t + Pi;
        }
        if ( msg->ranges[i] < (max_length) ) {
            if ( !t->dots.empty() ) {
                // 距离限制
                if ( abs(t->dots[t->dots.size() - 1].l - msg->ranges[i]) < 0.05 ) {
                    t->dots.emplace_back(msg->ranges[i], angle_t);
                }
                else {
                    lines.push_back(*t);
                    t = new line();
                    t->dots.emplace_back(msg->ranges[i], angle_t);
                }
            }
            else {
                t->dots.emplace_back(msg->ranges[i], angle_t);
            }
        }
        else {
            if ( t->dots.empty() ) {
                continue;
            }
            if ( abs(t->dots[t->dots.size() - 1].angle - angle_t) <=
                 3 * msg->angle_increment ) {
                continue;
            }
            else {
                lines.push_back(*t);
                t = new line();
            }
        }
    }
    if ( !t->dots.empty() ) {
        lines.push_back(*t);
    }
    if ( lines.empty() ) {
        return 0;
    }
    // 过零处理
    len = lines.size();
    if ( abs(lines[0].dots[0].l -
             lines[len - 1].dots[lines[len - 1].dots.size() - 1].l) < 0.1 &&
         lines[0].dots[0].angle > -1 * Pi / 10 &&
         lines[0].dots[0].angle < Pi / 100 &&
         lines[len - 1].dots[lines[len - 1].dots.size() - 1].angle < Pi / 10 &&
         lines[len - 1].dots[lines[len - 1].dots.size() - 1].angle >
             -1 * Pi / 100 ) {
        t = new line();
        for ( int i = lines[0].dots.size() - 1; i >= 0; i-- ) {
            t->dots.emplace_back(lines[0].dots[i].x, lines[0].dots[i].y, 1);
        }
        for ( int i = lines[len - 1].dots.size() - 1; i >= 0; i-- ) {
            t->dots.emplace_back(lines[len - 1].dots[i].x, lines[len - 1].dots[i].y, 1);
        }
        lines.erase(lines.begin());
        lines.erase(lines.end() - 1);
        lines.push_back(*t);
    }
    //**************************************************************************

    int t1 = 0;

    if ( type == SIDE ) {
        // 这里是侧方停车获取姿态的地方
        t1 = count_side_sign(lines);
    }
    else if ( type == HEAD || type == BACK ) {
        if ( flag == 0 ) {
            t1 = count_two_sign(lines);
        }
        else if ( flag == 1 ) {
            t1 = count_one_sign(lines);
        }
    }
    return t1;
}

inline int path::count_side_sign(vector<line>& lines)
{
    vector<line> t_lines;
    dot          PL, PR;
    // 寻找在范围内的线段并进行填充
    for ( auto& line : lines ) {
        if ( line.dots[0].angle < 0 && line.dots.size() >= 5 ) {
            line.fill_line();
            t_lines.push_back(line);
        }
    }
}

inline int path::count_one_sign(vector<line>& lines)
{
    vector<line> t_lines;
    line         PL;
    // 寻找在范围内的线段并进行填充
    if ( type == HEAD || type == BACK ) {
        for ( auto& line : lines ) {
            if ( line.dots[0].angle > (-Pi / 4) && line.dots[0].angle < (Pi / 4) ) {
                line.fill_line();
                t_lines.push_back(line);
            }
        }
    }
    // 检查t_lines的点数，并进行PR和PL赋值
    if ( t_lines.size() == 1 ) {
        PL = t_lines[0];
    }
    else {
        int    max   = 0;
        double max_y = 0;
        for ( int i = 0; i < t_lines.size(); i++ ) {
            if ( t_lines[i].dot_mid.y > max_y ) {
                max   = i;
                max_y = t_lines[i].dot_mid.y;
            }
        }
        PL = t_lines[max];
    }

    car_cur       = PL.dot_mid;
    car_cur.angle = atan(PL.k);
    if ( type == HEAD && car_cur.l < 0.455    ){//0.42
        flag = 2;
        return 0;
    }
    else if ( type == BACK && car_cur.l < 0.36 ) {
        flag = 2;
        return 0;
    }
    cout << car_cur << endl;
    return 1;
}

inline int path::count_two_sign(vector<line>& lines)
{
    vector<line> t_lines;
    dot          PL, PR;
    // 寻找在范围内的线段并进行填充
    for ( auto& line : lines ) {
        line.fill_line();
    }

    for ( auto& line : lines ) {
        if ( line.dots[0].angle > (-Pi / 3) && line.dots[0].angle < (Pi / 2) &&
             line.dots.size() >= 3 ) {
            if ( abs(line.k) < 1 ) {
                t_lines.push_back(line);
            }
        }
    }
    // 检查t_lines的点数，并进行PR和PL赋值
    if ( t_lines.size() <= 1 ) {
        // 这个真没办法处理，一般情况下都是出错了，直接返回0
        return 0;
    }
    else if ( t_lines.size() == 2 ) {
        if ( abs(t_lines[0].dots[0].y) > abs(t_lines[1].dots[0].y) ) {
            PL = t_lines[0].dot_mid;
            PR = t_lines[1].dot_mid;
        }
        else {
            PL = t_lines[1].dot_mid;
            PR = t_lines[0].dot_mid;
        }
    }
    else {
        // 寻找y距离相差不多的两个线进行处理
        // 基本确定一定能找到，就省去了异常处理
        for ( int i = 0; i < t_lines.size() - 1; i++ ) {
            double y1     = abs(t_lines[i].dot_mid.y) - abs(t_lines[i + 1].dot_mid.y);
            double angle1 = t_lines[i].k - t_lines[i + 1].k;
            if ( abs(y1) < 0.4 && abs(angle1) < 0.2 ) {
                if ( y1 > 0 ) {
                    PL = t_lines[i].dot_mid;
                    PR = t_lines[i + 1].dot_mid;
                }
                else {
                    PL = t_lines[i + 1].dot_mid;
                    PR = t_lines[i].dot_mid;
                }
                break;
            }
        }
    }

    // 矩阵求解参数，存储到matrix_canshu矩阵中
    Eigen::Matrix<double, 4, 1> matrix_canshu = mat_count(PL, PR);
    double                      square =
        matrix_canshu(0) * matrix_canshu(0) + matrix_canshu(1) * matrix_canshu(1);

    if ( abs(square - 1) <= 1 ) {
        car_cur.x = matrix_canshu(2);
        car_cur.y = matrix_canshu(3);
        car_cur.fill_dot();
        // 检测停车是否完成
        if ( sqrt((car_cur.x-0.1) * (car_cur.x-0.1) + (car_cur.y + 0.21) * (car_cur.y + 0.21)) <
             0.2 ) {
            flag = 1;
            vpoints.clear();
            dvp.clear();
            cout << "第一段停车完成了！！！！！！！！！！！！！！！！！！！！！！！！"
                 << endl;
            return 0;
        }

        car_cur.angle = asin(-1 * matrix_canshu(1));

        cout << car_cur << endl;
        cout << "square : " << square << endl;

        return 1;
    }
    else {
        cout << "\033[31msquare is not fit\033[0m" << endl;
    }
    return 0;
}

double path::trace_one_sign()
{
    return -0.55 * car_cur.angle;
}

double path::trace_two_sign()
{
    int    min = 0;
    double e   = 10000;
    double k   = 0.1;//1
    for ( int i = 0; i < dvp.size(); i++ ) {
        double dis = sqrt((car_cur.x - vpoints[i].x) * (car_cur.x - vpoints[i].x) + (car_cur.y - vpoints[i].y) * (car_cur.y - vpoints[i].y));
        if ( dis < e ) {
            e   = dis;
            min = i;
        }
    }

    double theta_e;

    if ( atan(dvp[min]) > 0 ) {
        theta_e = (Pi / 2 - atan(dvp[min])) - car_cur.angle;
    }
    else {
        theta_e = (-Pi / 2 - atan(dvp[min])) - car_cur.angle;
    }
        if (theta_e>0)return theta_e+atan(k*e);
        else return theta_e-atan(k*e);
    // return theta_e + atan(k * e);
}

Eigen::Matrix<double, 4, 1> path::mat_count(dot& PL, dot& PR)
{
    Eigen::Matrix<double, 4, 1> matrix_canshu;
    Eigen::Matrix<double, 4, 4> matrix_car;
    Eigen::Matrix<double, 4, 1> matrix_world;
    if ( type == HEAD ) {
        matrix_world << -0.0544, -0.334, 0.566, 0.190;
    }
    else if ( type == BACK ) {
        matrix_world << -0.041, 0.270, 0.541, 0.272;
    }
    else if ( type == SIDE ) {
        matrix_world << 0, 0, 0, 0;
    }
    matrix_car << PL.x, -PL.y, 1, 0, PR.x, -PR.y, 1, 0, PL.y, PL.x, 0, 1, PR.y,
        PR.x, 0, 1;
    //    matrix_car << PL.x, -PL.y, 1, 0, PR.x, -PR.y, 1, 0, PL.y, PL.x, 0, 1,
    //    PR.y,
    //        PR.x, 0, 1;
    matrix_canshu = matrix_car.inverse() * matrix_world;

    if ( type == HEAD || type == BACK ) {
        matrix_canshu(3) -= 0.21;   // 补偿
    }
    return matrix_canshu;
}

void path::generate_vpoints(int InsertNum)
{
    if ( vpoints.empty() )
        return;
    int Num = vpoints.size();

    int InsertNumSum = 0;   //  计算需要插入的点总数
    for ( int i = 0; i < Num - 1; i++ )
        InsertNumSum += InsertNum;

    //  将折线延长线上两点加入作为首点和尾点
    auto* temp = new dot[Num + 2];
    for ( int i = 0; i < Num; i++ )
        temp[i + 1] = vpoints[i];
    temp[0].x       = 2 * temp[1].x - temp[2].x;
    temp[0].y       = 2 * temp[1].y - temp[2].y;
    temp[Num + 1].x = 2 * temp[Num].x - temp[Num - 1].x;
    temp[Num + 1].y = 2 * temp[Num].y - temp[Num - 1].y;

    dot    NodePt1, NodePt2, NodePt3, NodePt4;
    double t;
    vpoints.resize(Num + InsertNumSum);

    int totalnum = 0;
    for ( int i = 0; i < Num - 1; i++ )   //  每条线段均匀插入点
    {
        NodePt1   = temp[i];
        NodePt2   = temp[i + 1];
        NodePt3   = temp[i + 2];
        NodePt4   = temp[i + 3];
        double dt = 1.0 / (InsertNum + 1);

        for ( int j = 0; j < InsertNum + 1; j++ ) {
            t                   = dt * j;
            vpoints[totalnum].x = F03(t) * NodePt1.x + F13(t) * NodePt2.x +
                                  F23(t) * NodePt3.x + F33(t) * NodePt4.x;
            vpoints[totalnum].y = F03(t) * NodePt1.y + F13(t) * NodePt2.y +
                                  F23(t) * NodePt3.y + F33(t) * NodePt4.y;
            totalnum++;
        }

        if ( i == Num - 2 ) {   //  最后一个尾点
            t                   = 1;
            vpoints[totalnum].x = F03(t) * NodePt1.x + F13(t) * NodePt2.x +
                                  F23(t) * NodePt3.x + F33(t) * NodePt4.x;
            vpoints[totalnum].y = F03(t) * NodePt1.y + F13(t) * NodePt2.y +
                                  F23(t) * NodePt3.y + F33(t) * NodePt4.y;
            totalnum++;
        }
    }
    delete[] temp;
}

void path::generate_dvp()
{
    // 生成导数队列
    int Num = vpoints.size();
    dvp.clear();
    dvp.resize(Num);
    for ( int i = 1; i < Num - 1; i++ ) {
        if ( abs(vpoints[i + 1].x - vpoints[i - 1].x) < 0.001 ) {
            dvp[i] = 50000;
        }
        else {
            dvp[i] = (vpoints[i + 1].y - vpoints[i - 1].y) /
                     (vpoints[i + 1].x - vpoints[i - 1].x);
        }
    }
    dvp[0]       = dvp[1];
    dvp[Num - 1] = dvp[Num - 2];
}

/**
 * \name LINE
 * \brief
 * 成员函数定义
 */
void line::fill_line()
{
    unsigned long long n = dots.size();

    double sumX = 0.0, sumY = 0.0, sumXY = 0.0, sumX2 = 0.0;
    for ( auto& i : dots ) {
        sumX += i.x;
        sumY += i.y;
        sumXY += i.x * i.y;
        sumX2 += i.x * i.x;
    }

    k = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
    b = (sumY - k * sumX) / n;
    double x_mid =
        (b * tan(dots[0].angle) / (1 - k * tan(dots[0].angle)) +
         b * tan(dots[n - 1].angle) / (1 - k * tan(dots[n - 1].angle))) /
        2;

    dot_mid = dot(x_mid, k * x_mid + b, -1);
}
